/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/robina_tiny_communication/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace robina_tiny_communication {

    /*****************************************************************************
** Implementation
*****************************************************************************/

    QNode::QNode(int argc, char** argv, QHostAddress _ip, int _send_port, int _receive_port ) :
            init_argc(argc),
            init_argv(argv)
    {
        udpSocket       = new QUdpSocket;
        resultUdpSocket = new QUdpSocket;

        ip = _ip;
        send_port = _send_port;
        receive_port = _receive_port;

        resultUdpSocket->bind(receive_port);

        udpSocket->connectToHost(ip, send_port);

    }

    QNode::~QNode()
    {
        if(ros::isStarted())
        {
            ros::shutdown(); // explicitly needed since we use ros::start();
            ros::waitForShutdown();
        }
	wait();
    }

    bool QNode::init()
    {
	ros::init(init_argc,init_argv,"robina_tiny_communication");

        if ( ! ros::master::check() )
        {
            return false;
	}
        ros::start();
	    ros::NodeHandle n;
        ROS_INFO("Initializing....");

        tiny_2dNav_marker_as = new tiny_2dNav_marker_server(n, "/tiny_2dNav_marker",
                                                            boost::bind( &QNode::tiny_2dNav_marker_executeCb, this, _1), false);
        tiny_2dNav_marker_as->start();

        tiny_2dNav_pos_as = new tiny_2dNav_pos_server(n, "/tiny_2dNav_pos",
                                                      boost::bind(&QNode::tiny_2dNav_pos_executeCb, this, _1), false);
        tiny_2dNav_pos_as->start();

        tiny_spot_as = new tiny_spot_server (n, "/tiny_spot",
                                                         boost::bind(&QNode::tiny_spot_executeCb, this, _1), false);
        tiny_spot_as->start();

        tiny_go_and_spot_as = new tiny_go_and_spot_server(n, "tiny_go_and_spot_pos",
                                                          boost::bind(&QNode::tiny_go_and_spot_executeCb, this, _1), false);
        tiny_go_and_spot_as->start();

        tiny_go_and_spot_marker_as = new tiny_go_and_spot_marker_server(n, "tiny_go_and_spot_marker",
                                                                 boost::bind(&QNode::tiny_go_and_spot_marker_executeCb, this, _1), false);
        tiny_go_and_spot_marker_as->start();

        tiny_introduction_ac = new tiny_introduction_server (n, "tiny_introduction",
                                                             boost::bind(&QNode::tiny_introduction_executeCb, this, _1), false);
        tiny_introduction_ac->start();


        ROS_INFO("Initialization finished");

        start();
    }

    void QNode::run()
    {
        ros::Rate loop_rate(10);
	int count = 0;
        while ( ros::ok() )
        {
            ros::spinOnce();
            loop_rate.sleep();
            ++count;
        }
        std::cout << "Ros shutdown " << std::endl;
        Q_EMIT rosShutDown();
    }

    void QNode::tiny_2dNav_marker_executeCb(const robina_tiny_communication::tiny_2dnav_markerGoalConstPtr &goal)
    {
        ROS_INFO("request for marker received");
        marker = goal->goal_name;

        QString marker_goal = QString::fromStdString(marker.c_str());

        generate_2dNavMarkerXml(marker_goal);
    }

    void QNode::tiny_2dNav_pos_executeCb(const robina_tiny_communication::tiny_2dnav_posGoalConstPtr &goal)
    {
        ROS_INFO("request for position receives");
        pos.pose = goal->position.pose;
        generate_2dNavGoalXml(pos);
    }

    void QNode::tiny_spot_executeCb(const robina_tiny_communication::tiny_spotGoalConstPtr &goal)
    {
        ROS_INFO("request for spot received");
        spot = goal->goal;
        generate_setSpotXml(spot);
    }

    void QNode::tiny_go_and_spot_executeCb(const robina_tiny_communication::tiny_go_and_spot_posGoalConstPtr &goal)
    {
        generate_2dNavGoalXml(goal->position);
        generate_setSpotXml(true);
    }
    void QNode::tiny_go_and_spot_marker_executeCb(const robina_tiny_communication::tiny_go_and_spot_markerGoalConstPtr &goal)
    {
        generate_2dNavMarkerXml(QString::fromStdString(goal->goal_name));
        generate_setSpotXml(true);
    }

    void QNode::tiny_introduction_executeCb(const robina_tiny_communication::tiny_introductionGoalConstPtr &goal)
    {
        generate_2dNavMarkerXml(QString::fromStdString(goal->goal_name));
        generate_introductionXml(QString::fromStdString("introduction"));
    }

    QByteArray QNode::generate_introductionXml(QString introduction)
    {
        QByteArray byteArray;
        QXmlStreamWriter xml_writer(&byteArray);
        xml_writer.writeStartDocument();
        xml_writer.writeStartElement("robina");
        xml_writer.writeStartElement("command");
        xml_writer.writeTextElement ("introduction",introduction);
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndDocument();

        ROS_INFO(byteArray);

        sendXml(byteArray);

        return byteArray;
    }

    QByteArray QNode::generate_2dNavMarkerXml(QString marker_goal)
    {
        QByteArray byteArray;
        QXmlStreamWriter xml_writer(&byteArray);
        xml_writer.writeStartDocument();
        xml_writer.writeStartElement("robina");
        xml_writer.writeStartElement("command");
        xml_writer.writeTextElement ("marker",marker_goal);
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndDocument();

        ROS_INFO(byteArray);

        sendXml(byteArray);

        return byteArray;
    }

    QByteArray QNode::generate_2dNavGoalXml(geometry_msgs::PoseStamped poseStamped)
    {
        QByteArray byteArray;
        QXmlStreamWriter xml_writer(&byteArray);
        xml_writer.writeStartDocument();
        xml_writer.writeStartElement("robina");
        xml_writer.writeStartElement("command");
        xml_writer.writeStartElement("navigation");
        xml_writer.writeStartElement("position");
        xml_writer.writeTextElement("x",QString::number( poseStamped.pose.position.x) );
        xml_writer.writeTextElement("y",QString::number( poseStamped.pose.position.y) );
        xml_writer.writeTextElement("z",QString::number( poseStamped.pose.position.z) );
        xml_writer.writeEndElement();
        xml_writer.writeStartElement("orientation");
        xml_writer.writeTextElement("x",QString::number( poseStamped.pose.orientation.x) );
        xml_writer.writeTextElement("y",QString::number( poseStamped.pose.orientation.y) );
        xml_writer.writeTextElement("z",QString::number( poseStamped.pose.orientation.z) );
        xml_writer.writeTextElement("w",QString::number( poseStamped.pose.orientation.w) );
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndDocument();

        ROS_INFO(byteArray);

        sendXml(byteArray);

        return byteArray;
    }

    QByteArray QNode::generate_armGoToHomeXml()
    {
        QByteArray byteArray;
        QXmlStreamWriter xml_writer(&byteArray);
        xml_writer.writeStartDocument();
        xml_writer.writeStartElement("robina");
        xml_writer.writeStartElement("command");
        xml_writer.writeEmptyElement(QString("armGotoHome"));
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndDocument();

        sendXml(byteArray);

        return byteArray;
    }

    QByteArray QNode::generate_setSpotXml(bool enable)
    {
        QByteArray byteArray;
        QXmlStreamWriter xml_writer(&byteArray);
        xml_writer.writeStartDocument();
        xml_writer.writeStartElement("robina");
        xml_writer.writeStartElement("command");
        xml_writer.writeTextElement("spot",(enable)?"1":"0");
        xml_writer.writeEndElement();
        xml_writer.writeEndElement();
        xml_writer.writeEndDocument();

        qDebug()<<byteArray;

        sendXml(byteArray);

        return byteArray;
    }


    QByteArray QNode::readPendingDatagrams()
    {

        QByteArray datagram;
        while (resultUdpSocket->hasPendingDatagrams())
        {
            datagram.resize(resultUdpSocket->pendingDatagramSize());
            QHostAddress sender;
            quint16 senderPort;

            resultUdpSocket->readDatagram(datagram.data(), datagram.size(),
                                    &sender, &senderPort);
        }
        return datagram;
    }

    void QNode::sendXml(QByteArray xml)
    {
        udpSocket->writeDatagram(xml,ip,send_port);
        getResult();
    }

    bool QNode::parseXml( QByteArray read_xml )
    {
        std::string tiny_state;
        qDebug()<<"PARSING RECEIVED XML";
        qDebug()<<read_xml;

        QXmlStreamReader xml(read_xml);
        QXmlStreamReader::TokenType tokenType = xml.readNext();

        while(tokenType != QXmlStreamReader::StartElement)
            tokenType = xml.readNext();

        if(tokenType == QXmlStreamReader::StartElement)
        {
            if(xml.name() == "tiny")
            {
                xml.readNext();
                if(xml.name() == "actionResult")
                {
                    xml.readNext();
                    if(xml.name() == "state")
                    {
                        tiny_state = xml.readElementText().toStdString();
                    }
                }
            }
        }

        qDebug()<<tiny_state.c_str();

        if(tiny_state == "SUCCEEDED")
        {
            return true;
        }
        else
        {
            if(tiny_state == "ABORTED")
            {
                return false;
            }
        }
    }

    bool QNode::getResult()
    {
        resultUdpSocket->waitForReadyRead(30000);
        if(!resultUdpSocket->hasPendingDatagrams())
            return false;

            return parseXml(readPendingDatagrams());
    }

}  // namespace robina_tiny_communication
